#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "tf/transform_listener.h"

//This node looks up a transform and then uses this transform to generate a cmd_vel for the robot so that the robot follows the transform

const double max_speed_x = 0.1; // m/s
const double max_speed_y = 0.05; // m/s
const double max_rotation = 0.2; // rad/s
const double minimal_distance_x = 0.1;
const double minimal_distance_y = 0.2;

template <typename T> int sgn(T val) {
    return (T(0) < val) - (val < T(0));
}

int main(int argc, char **argv) {
  	ros::init(argc, argv, "biped_goalpose");
	ros::NodeHandle pn ("~");
	
	tf::TransformListener tfListener;

	ros::Publisher pub = pn.advertise<geometry_msgs::Twist>("cmd_vel", 10);

	std::string carrotFrame = "/carrot";
	std::string baseFrame = "/base_footprint";
	double carrot_distance_x;
	pn.param("carrot_frame_id", carrotFrame, carrotFrame);
	pn.param("base_frame_id", baseFrame, baseFrame);
	pn.param("carrot_distance_x", carrot_distance_x, -0.6);
	
	ros::Rate loop_rate(100.0); //publishen the Twist message with 100 Hz
	
	while (pn.ok()) {
		tf::StampedTransform transform;
		double dx, dy, da;
		
		ros::Time now = ros::Time::now();
		
		try {
			tfListener.waitForTransform(baseFrame, carrotFrame, now-ros::Duration(0.5), ros::Duration(0.5)); //makes sure that the carrot was detected in the last 0.5s
			tfListener.lookupTransform(baseFrame, carrotFrame, ros::Time(0), transform); //uses the last available carrot transform
			dx = transform.getOrigin().x() + carrot_distance_x;
			dy = transform.getOrigin().y();
			da = atan2(dy, dx);
		} catch (tf::TransformException ex) {
			//ROS_ERROR("%s",ex.what());
			ROS_INFO("Carrot not found! Please move the carrot into the field of view of the robot!");
			dx = 0;
			dy = 0;
			da = 0;
		}

		ROS_INFO("Carrot is at position  x:%f, y:%f", dx, dy);
		
		if (fabs(dx) < minimal_distance_x) dx = 0;
		if (fabs(dy) < minimal_distance_y) dy = 0;
		da = atan2(dy, dx);
		
		geometry_msgs::Twist cmd_vel;
		cmd_vel.linear.x = sgn(dx) * fmin(fabs(dx)*0.5, max_speed_x);
		cmd_vel.linear.y = sgn(dy) * fmin(fabs(dy)*0.5, max_speed_y)*0;
		cmd_vel.angular.z = sgn(da) * fmin(fabs(da), max_rotation);
		
		pub.publish(cmd_vel);
		
		loop_rate.sleep();
		ros::spinOnce();
	}

	return 0;
}

